import rclpy
from rclpy.node import Node

from carsource_interfaces.msg import GPSInterface

from nav_msgs.msg import Path


class GpsData:
    def __init__(self):
        self.x_pos = 0.0
        self.y_pos = 0.0
        self.anchor = []
        self.base_station = []
        self.time = 0.0
        self.fram_id = ''


class ImuData:
    def __init__(self):
        self.x_pos = 0.0
        self.y_pos = 0.0
        self.anchor = []
        self.base_station = []
        self.time = 0.0
        self.fram_id = ''


class FusionNode(Node):
    def __init__(self,name):
        super.__init__(name)

        self.gps_data = []
        self.imu_data = []
        self.car_ori_data = []
        #subscriber
        self.current_uwb_pos_sub = self.create_subscription(GPSInterface, "gps_data",self.recvgps,100000)
        self.current_imu_sub = self.create_subscription(GPSInterface, "imu_data",self.recvimu,100000)
        self.current_uwb_pos_sub = self.create_subscription(GPSInterface, "car_ori_data",self.recvcarori,100000)
        
        
    def recvgps(self, msg:GPSInterface):

        gps = GpsData()
        gps.time = msg.time
        gps.fram_id = msg.frame_id
        gps.x_pos = msg.x_pos
        gps.y_pos = msg.y_pos
        gps.anchor = msg.anchor
        gps.base_station = msg.base_station


        self.gps_data.append(gps)


def main(args=None):
    
    rclpy.init(args=args)

    fusion_node = FusionNode("fusion_node")

    rclpy.spin(fusion_node)

    rclpy.shutdown()

